# -*- coding: utf-8 -*-
import time
import serial
from collections import deque


# 香港理工机械手控制
class Exoskeleton(object):
    def __init__(self):
        self.command_num = 0
        self.Connected = False

    def connect(self, config_data):
        highest_point = config_data.exoskeleton['highestPoint']
        range = config_data.exoskeleton['lowestPoint'] - highest_point
        velocity = config_data.exoskeleton['velocity']
        com_num = config_data.comNumSet[config_data.affectedSide]
        if not self.Connected:
            self.link_to_exoskeleton(com_num, 9600)
            self.Connected = True
        if config_data.affectedSide == 'left':
            self.init_param(highest_point, range, velocity, True)
        else:
            self.init_param(highest_point, range, velocity, False)
        self.reset_arm()

    # 连接串口，输入串口号和波特率
    def link_to_exoskeleton(self, com_num, baud_rate):
        print('Trying to Connect to COM')
        try:
            self.COM = serial.Serial('COM' + str(com_num), baud_rate)
            print('Connect to COM successfully')
            self.goal = 1
            self.last = 0
            self.count = 0
            self.command_num = 0
            self.last_command = 'rest'
            self.command_length = 3
            self.epoch_command = deque(maxlen=self.command_length)
            self.epoch_command.extend([1, 1, 1])
        except Exception as e:
            print('Open serial failed.' + str(e))

    # 初始化外骨骼的速度，最高和最低点位置
    def init_param(self, start, range, speed, is_left):
        self.speed = speed
        self.is_left = is_left
        if is_left:
            self.min = int(2500 - start * (4000.0 / 360.0))
            self.max = self.min - int(range * (4000.0 / 360.0))
        else:
            self.min = int(1400 + start * (4000.0 / 360.0))
            self.max = self.min + int(range * (4000.0 / 360.0))

    # 读取当前位置
    def read_position(self):
        self.COM.write("APE".encode())
        time.sleep(0.01)
        position = int(self.COM.read(4).decode())
        self.COM.read_all()
        return position

    # 机械手分阶段运动
    def smooth_command(self, command, stage):
        self.epoch_command.append(command)
        rest_sum = sum(self.epoch_command)  # 0:move|1:rest
        if stage == 1:
            stage_func = self.stretch_arm
        else:
            stage_func = self.back_arm
        self.send_epoch_command(stage, rest_sum)

    def send_epoch_command(self, stage, rest_sum):
        if rest_sum == 0 and self.last_command == 'rest':
            if stage == 1:
                self.stretch_arm()
            else:
                self.back_arm()
            self.last_command = 'move'
        elif rest_sum == self.command_length and self.last_command == 'move':
            self.exoskeleton_stop(stage)
            self.last_command = 'rest'

    # 机械手循环运动
    def strategy_reciprocating(self):
        now_position = self.read_position()
        if abs(self.last - now_position) == 0:
            if self.goal == 1:
                self.stretch_arm()
                self.goal = 2
            else:
                self.goal = 1
                self.back_arm()
        self.last = now_position

    # 预览
    def preview_step(self):
        i = 0
        while i < 20:
            self.strategy_reciprocating()
            time.sleep(0.2)
            i += 1
        # self.back_arm()

    # 停止运动
    def exoskeleton_stop(self, stage):
        if (self.is_left and stage == 1) or (not self.is_left and stage != 1):
            position_diff = -20
        else:
            position_diff = 20
            
        position = self.read_position()
        stop_command = "AS"+str(self.speed)+"G" + str(position + position_diff) + "E"
        self.COM.write(stop_command.encode())

    # 展臂动作
    def stretch_arm(self):
        stretch_command = "AS" + str(self.speed) + "G" + str(self.max) + "E"
        self.COM.write(stretch_command.encode())
        print("stretch")
    # 收臂动作
    def back_arm(self):
        back_command = "AS" + str(self.speed) + "G" + str(self.min) + "E"
        self.COM.write(back_command.encode())
        print("back")

    # 快速复位动作
    def reset_arm(self):
        reset_command = "AS" + str(self.speed + 20) + "G" + str(self.min) + "E"
        self.COM.write(reset_command.encode())
        print("reset")

    # 关闭串口
    def disconnect_com(self):
        self.stretch_arm()
        # time.sleep(0.1)
        self.COM.close()
        print("Exoskeleton close successfully")
        self.Connected = False


if __name__ == "__main__":
    ES = Exoskeleton()
    comNum = 38
    baudRate = 9600
    ES.link_to_exoskeleton(comNum, baudRate)
    ES.init_param(0, 80, 30, True)

    time.sleep(2)
    ES.COM.write("AS20G2500E")
    testlist = [
        'move', 'move', 'move', 'rest', 'rest', 'rest', 'move', 'rest', 'rest', 'rest', \
        'rest', 'move', 'rest', 'move', 'move', 'rest', 'move', 'rest', 'move', 'rest', 'rest', 'move', \
        'rest', 'move']
    ES.reset_arm()
    time.sleep(2)
    # for i in range(len(testlist)):
    #     ES.ExoskeletonStop()
    #     time.sleep(0.1)
    for i in range(len(testlist)):
        ES.strategy_epoch(testlist[i])
        print(i + 1)
        time.sleep(0.5)
        # if i% 5 == 0:
            # ES.Reset()
    for i in range(len(testlist)):
        ES.strategy_epoch(testlist[i])
        print(i + 25)
        time.sleep(0.02)
    ES.disconnect_com()
